' Mini_Sumo_3.2_Line_Sensor_Read.BS2
' {$STAMP BS2}
' {$PBASIC 2.5}

'-----[ I/O Definitions ]------------------------------------------------

LLinePwr       PIN    10                 ' left line sensor power
LLineIn        PIN     9                 ' left line sensor input
RLinePwr       PIN     7                 ' right line sensor power
RLineIn        PIN     8                 ' right line sensor input

'-----[ Variables ]------------------------------------------------------

lLine          VAR    Word               ' left sensor raw reading
rLine          VAR    Word               ' right sensor raw reading
lineBits       VAR    Nib                ' decoded sensors value
lbLeft         VAR    lineBits.BIT1
lbRight        VAR    lineBits.BIT0

'-----[ Program Code ]---------------------------------------------------

Main:
  GOSUB Read_Line_Sensors
  DEBUG HOME, "LR", CR,                  ' show sensor reading
        BIN2 lineBits, CR, CR

  SELECT lineBits                        ' display actions
    CASE %00
      DEBUG "Continue forward", CLREOL
    CASE %01
      DEBUG "Spin Left", CLREOL
    CASE %10
      DEBUG "Spin Right", CLREOL
    CASE %11
      DEBUG "Back up and turn around", CLREOL
  ENDSELECT

  GOTO Main
  END

'-----[ Subroutines ]----------------------------------------------------

Read_Line_Sensors:
  HIGH LLinePwr                          ' activate sensors
  HIGH RLinePwr
  HIGH LLineIn                           ' discharge caps
  HIGH RLineIn
  PAUSE 1
  RCTIME LLineIn, 1, lLine               ' read left sensor
  RCTIME RLineIn, 1, rLine               ' read right sensor
  LOW LLinePwr                           ' deactivate sensors
  LOW RLinePwr

  ' convert readings to bits
  LOOKDOWN lLine, >=[1000, 0], lbLeft    ' 0 = black, 1 = line
  LOOKDOWN rLine, >=[1000, 0], lbRight
  RETURN